import open3d as o3d
import numpy as np
import cv2
import sys
import json
import time
import os
from interact import demo_manual_registration, demo_crop_geometry

def pcd_to_numpy(pcd):
    """
    将PCD文件转换为NumPy数组。
    
    参数:
        pcd_path (str): PCD文件的路径。
    
    返回:
        numpy.ndarray: 形状为(N, 6)的数组，其中N是点云中的点数。
                    前三列是点的坐标(x, y, z)，后三列是颜色信息(r, g, b)。
                    如果PCD文件中没有颜色信息，则后三列将填充为0。
    """
    
    # 获取点云的坐标
    points = np.asarray(pcd.points)  # (N, 3) 数组，包含点云的坐标
    
    # 获取点云的颜色
    if pcd.has_colors():
        colors = np.asarray(pcd.colors)  # (N, 3) 数组，包含点云的颜色
    else:
        colors = np.zeros_like(points)  # 如果没有颜色信息，填充为0
    
    # 将坐标和颜色合并为一个 (N, 6) 的数组
    combined = np.hstack((points, colors))
    
    return combined

def numpy_to_pcd(numpy_array):
    """
    将NumPy数组转换为PCD文件。
    
    参数:
        numpy_array (numpy.ndarray): 形状为(N, 6)的数组，其中N是点云中的点数。
                                    前三列是点的坐标(x, y, z)，后三列是颜色信息(r, g, b)。
        pcd_path (str): 输出PCD文件的路径。
    """
    # 确保输入数组的形状正确
    if numpy_array.shape[1] != 6:
        raise ValueError("输入数组的形状必须是 (N, 6)，其中N是点云中的点数。")
    
    # 分离坐标和颜色
    points = numpy_array[:, :3]  # (N, 3) 数组，包含点云的坐标
    colors = numpy_array[:, 3:]  # (N, 3) 数组，包含点云的颜色
    
    # 创建Open3D点云对象
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(points)
    pcd.colors = o3d.utility.Vector3dVector(colors)
    
    return pcd

def mask_pcd(sim_pcd, real_pcd, trans_mat):
    """
    点云注入

    参数:
        sim_pcd (o3d.geometry.PointCloud): 源点云。
        real_pcd (o3d.geometry.PointCloud): 目标点云。
        trans_mat (numpy.ndarray): 变换矩阵。

    返回:
        o3d.geometry.PointCloud: 应用点云注入后的点云。
    """
    

if __name__ == "__main__":
    pass